﻿/// <summary>
/// Developer: CHOOI KAH WAI
/// Last Modified: 13/10/2009
/// </summary>

using System;
using System.Collections.Generic;
using System.Collections;
using System.Runtime.InteropServices;
using ARSS.Components;
using ARSS.Util;
using jp.nyatla.nyartoolkit.cs;
using jp.nyatla.nyartoolkit.cs.core;
using jp.nyatla.nyartoolkit.cs.detector;
using NyARToolkitCSUtils.NyAR;


namespace ARSS.Components
{
    /// <summary>
    /// NyAR Component
    /// Implementation of NyAR was refered to NyARToolkitCS(for C#)
    /// NyARToolkitCS(for C#) Copyright (C)2008 R.Iizuka
    /// NyARToolkitCS(for C#) can be obtained from http://nyatla.jp/nyartoolkit/wiki/index.php?NyARToolkitCS
    /// </summary>
    public class ARCom_NyAR : ARCom
    {
        #region Variables

        /// <summary>
        /// Camera calibration data
        /// </summary>
        private const string camFile = "data/camera_para.dat";

        /// <summary>
        /// Marker detector
        /// </summary>
        private NyARSingleDetectMarker markerDetector;

        /// <summary>
        /// Video raster
        /// </summary>
        private DsBGRX32Raster raster;

        /// <summary>
        /// Minimum view distance
        /// </summary>
        private double minViewDistance = 1;

        /// <summary>
        /// Maximum view distance
        /// </summary>
        private double maxViewDistance = 1000;

        #endregion

        #region Functions

        /// <summary>
        /// Calculate camera frustrum
        /// This section refered to NyARToolkitCS for the formula
        /// </summary>
        /// <param name="_arParam">AR parameter</param>
        /// <returns>Camera frustrum matrix</returns>
        protected double[] GetCameraFrustum(NyARParam arParam)
        {
            double[] projectionMatrix = new double[16];

            NyARMat trans_mat = new NyARMat(3, 4);
            NyARMat icpara_mat = new NyARMat(3, 4);
            double[,] p = new double[3, 3], q = new double[4, 4];
            int width, height;
            int i, j;

            NyARIntSize size = arParam.getScreenSize();
            width = size.w;
            height = size.h;

            arParam.getPerspectiveProjectionMatrix().decompMat(icpara_mat, trans_mat);

            double[][] icpara = icpara_mat.getArray();
            double[][] trans = trans_mat.getArray();
            for (i = 0; i < 4; i++)
            {
                icpara[1][i] = (height - 1) * (icpara[2][i]) - icpara[1][i];
            }

            for (i = 0; i < 3; i++)
            {
                for (j = 0; j < 3; j++)
                {
                    p[i, j] = icpara[i][j] / icpara[2][2];
                }
            }
            q[0, 0] = (2.0 * p[0, 0] / (width - 1));
            q[0, 1] = (2.0 * p[0, 1] / (width - 1));
            q[0, 2] = ((2.0 * p[0, 2] / (width - 1)) - 1.0);
            q[0, 3] = 0.0;

            q[1, 0] = 0.0;
            q[1, 1] = -(2.0 * p[1, 1] / (height - 1));
            q[1, 2] = -((2.0 * p[1, 2] / (height - 1)) - 1.0);
            q[1, 3] = 0.0;

            q[2, 0] = 0.0;
            q[2, 1] = 0.0;
            q[2, 2] = (this.maxViewDistance + this.minViewDistance) / (this.minViewDistance - this.maxViewDistance);
            q[2, 3] = 2.0 * this.maxViewDistance * this.minViewDistance / (this.minViewDistance - this.maxViewDistance);

            q[3, 0] = 0.0;
            q[3, 1] = 0.0;
            q[3, 2] = -1.0;
            q[3, 3] = 0.0;

            projectionMatrix[0] = (float)(q[0, 0] * trans[0][0] + q[0, 1] * trans[1][0] + q[0, 2] * trans[2][0]);
            projectionMatrix[1] = (float)(q[1, 0] * trans[0][0] + q[1, 1] * trans[1][0] + q[1, 2] * trans[2][0]);
            projectionMatrix[2] = (float)(q[2, 0] * trans[0][0] + q[2, 1] * trans[1][0] + q[2, 2] * trans[2][0]);
            projectionMatrix[3] = (float)(q[3, 0] * trans[0][0] + q[3, 1] * trans[1][0] + q[3, 2] * trans[2][0]);
            projectionMatrix[4] = (float)(q[0, 1] * trans[0][1] + q[0, 1] * trans[1][1] + q[0, 2] * trans[2][1]);
            projectionMatrix[5] = (float)(q[1, 1] * trans[0][1] + q[1, 1] * trans[1][1] + q[1, 2] * trans[2][1]);
            projectionMatrix[6] = (float)(q[2, 1] * trans[0][1] + q[2, 1] * trans[1][1] + q[2, 2] * trans[2][1]);
            projectionMatrix[7] = (float)(q[3, 1] * trans[0][1] + q[3, 1] * trans[1][1] + q[3, 2] * trans[2][1]);
            projectionMatrix[8] = (float)(q[0, 2] * trans[0][2] + q[0, 1] * trans[1][2] + q[0, 2] * trans[2][2]);
            projectionMatrix[9] = (float)(q[1, 2] * trans[0][2] + q[1, 1] * trans[1][2] + q[1, 2] * trans[2][2]);
            projectionMatrix[10] = (float)(q[2, 2] * trans[0][2] + q[2, 1] * trans[1][2] + q[2, 2] * trans[2][2]);
            projectionMatrix[11] = (float)(q[3, 2] * trans[0][2] + q[3, 1] * trans[1][2] + q[3, 2] * trans[2][2]);
            projectionMatrix[12] = (float)(q[0, 3] * trans[0][3] + q[0, 1] * trans[1][3] + q[0, 2] * trans[2][3] + q[0, 3]);
            projectionMatrix[13] = (float)(q[1, 3] * trans[0][3] + q[1, 1] * trans[1][3] + q[1, 2] * trans[2][3] + q[1, 3]);
            projectionMatrix[14] = (float)(q[2, 3] * trans[0][3] + q[2, 1] * trans[1][3] + q[2, 2] * trans[2][3] + q[2, 3]);
            projectionMatrix[15] = (float)(q[3, 3] * trans[0][3] + q[3, 1] * trans[1][3] + q[3, 2] * trans[2][3] + q[3, 3]);

            return projectionMatrix;
        }

        #endregion

        #region ARCom Implementations

        #region Delegates / Events

        public override event ARSS.Components.Coordinator.ARUpdateHandler ARUpdateEvent;

        #endregion

        #region Functions

        public override void Init(int vidWdth, int vidHeight, string boardCfg, string actionCfg, Coordinator co, CamVideoInfo cam)
        {
            CalcResults = new ArrayList();

            // Initialize NyAR Engine
            NyARParam param = new NyARParam();
            param.loadARParamFromFile(camFile);

            param.changeScreenSize(vidWdth, vidHeight);

            // Align Camera
            double[] camMatrix = GetCameraFrustum(param);
            co.AlignCamera(camMatrix);

            NyARCode code = new NyARCode(16, 16);
            code.loadARPattFromFile(boardCfg);

            this.markerDetector = new NyARSingleDetectMarker(param, code, 25.0);
            this.markerDetector.setContinueMode(true);

            this.raster = new DsBGRX32Raster(cam.Width, cam.Height, cam.Stride);

            // Subscribe to camera update
            co.SubscribeCameraUpdateHandler(new Coordinator.CameraUpdateHandler(OnCameraUpdate));
        }

        public override void OnCameraUpdate(byte[] _buffer)
        {
            // Convert byte[] to IntPtr
            GCHandle handle = GCHandle.Alloc(_buffer, GCHandleType.Pinned);
            this.raster.setBuffer(handle.AddrOfPinnedObject(), true);
            handle.Free();


            CalcResults.Clear();
            if (this.markerDetector.detectMarkerLite(this.raster, 150) &&
                this.markerDetector.getConfidence() > 0.4)
            {
                // Marker detected
                NyARTransMatResult mat = new NyARTransMatResult();
                this.markerDetector.getTransmationMatrix(mat);

                double[] dMat = new double[12];
                mat.getValue(dMat);

                ARMarker marker = new ARMarker();
                marker.ID = -1;
                marker.TransMatrix = MatrixUtil.GetDMatrixFromNyARTransMat(dMat);

                // Add results to array
                CalcResults.Add(marker);

                // Fire AR update event
                ARUpdateEvent(CalcResults);
            }
        }

        public override void Destroy(Coordinator co)
        {
            co.UnSubscribeCameraUpdateHandler(new Coordinator.CameraUpdateHandler(OnCameraUpdate));
        }
        #endregion

        #endregion

    }
}
